#include "Nano100Series.h"
#include "HmdCmd.h"
#include "MotionSensor.h"
#include "Common.h"
#include "Config.h"
#include "EventQueue.h"
#include "TouchSensor.h"

#define HMD_CMD_GSENSOR_START	(0x00)
#define HMD_CMD_GSENSOR_STOP	(0x01)
#define HMD_CMD_GSENSOR_POLL	(0x02)

#define HMD_CMD_GYRO_START		(0x00)
#define HMD_CMD_GYRO_STOP		(0x01)
#define HMD_CMD_GYRO_POLL		(0x02)

#define HMD_CMD_MAGNETIC_START	(0x00)
#define HMD_CMD_MAGNETIC_STOP	(0x01)
#define HMD_CMD_MAGNETIC_POLL	(0x02)
#define HMD_CMD_DISP_XFER		(0x03)
#define HMD_CMD_EVENT_POLL		(0x04)

#define HMD_CMD_LENGTH_MAX		(63)

#define HMD_CMD_DISP_XFER_SYNC_BEGIN			(0x55)
#define HMD_CMD_DISP_XFER_SYNC_END				(0xAA)

#define HMD_CMD_DISP_XFER_RESULT_OK				(0)
#define HMD_CMD_DISP_XFER_RESULT_ERROR			(-1)

#define HMD_CMD_DISP_XFER_GET_CONTRAST				(0x00)
#define HMD_CMD_DISP_XFER_SET_CONTRAST				(0x01)
#define HMD_CMD_DISP_XFER_GET_BRIGHTNESS			(0x02)
#define HMD_CMD_DISP_XFER_SET_BRIGHTNESS			(0x03)
#define HMD_CMD_DISP_XFER_GET_SATURATION			(0x04)
#define HMD_CMD_DISP_XFER_SET_SATURATION			(0x05)
#define HMD_CMD_DISP_XFER_GET_3D					(0x06)
#define HMD_CMD_DISP_XFER_SET_3D					(0x07)
#define HMD_CMD_DISP_XFER_GET_VIDEO_STATE			(0x08)
#define HMD_CMD_DISP_XFER_GET_VERSION				(0x09)
#define HMD_CMD_DISP_XFER_GET_BRIGHTNESS_OFFSET		(0x0A)
#define HMD_CMD_DISP_XFER_SET_BRIGHTNESS_OFFSET		(0x0B)
#define HMD_CMD_DISP_XFER_GET_BRIGHTNESS_ABS		(0x0C)
#define HMD_CMD_DISP_XFER_SET_BRIGHTNESS_ABS		(0x0D)

#define HMD_PARAM_DISP_3D_NONE			(0x00)
#define HMD_PARAM_DISP_3D_HALF_LR		(0x01)
#define HMD_PARAM_DISP_3D_HALF_TB		(0x02)
#define HMD_PARAM_DISP_3D_FULL			(0x03)

#define HMD_PARAM_DISP_VIDEO_STATE_OFF	(0x00)
#define HMD_PARAM_DISP_VIDEO_STATE_ON	(0x01)

#define FRACTOR_SCALE_4		(10000)


void HmdRspNull(uint8_t *cmd_buf, uint32_t cmd_size, uint8_t *rsp_buf, uint32_t rsp_size, uint32_t *rsp_ret_size)
{
	*rsp_ret_size = 0;
}

/*
 * buf x store device y
 * buf y store device x
 * buf z store device -z
 */
void HmdGsensorRspPoll(uint8_t *rsp_buf, uint32_t *rsp_size)
{
	int x = 0;
	int y = 0;
	int z = 0;
	uint8_t buf_raw[6];

	MotionSensorGetAcc(buf_raw, sizeof(buf_raw));
	x = buf_raw[0];
	x |= buf_raw[1] << 8;
	x <<= 16;
	x >>= 16;
	//LogDebug("Acc : x = %d ", x);
	x >>= 4;

	y = buf_raw[2];
	y |= buf_raw[3] << 8;
	y <<= 16;
	y >>= 16;
	//LogDebug("y = %d ", y);
	y >>= 4;

	z = buf_raw[4];
	z |= buf_raw[5] << 8;
	z <<= 16;
	z >>= 16;
	//LogDebug("z = %d\n", z);
	z >>= 4;

	x = -x;
	rsp_buf[0] = x & 0xFF;
	rsp_buf[1] = (x >> 8) & 0xFF;
	rsp_buf[2] = (x >> 16) & 0xFF;
	rsp_buf[3] = (x >> 24) & 0xFF;

	rsp_buf[4 + 0] = y & 0xFF;
	rsp_buf[4 + 1] = (y >> 8) & 0xFF;
	rsp_buf[4 + 2] = (y >> 16) & 0xFF;
	rsp_buf[4 + 3] = (y >> 24) & 0xFF;

	z = -z;
	rsp_buf[8 + 0] = z & 0xFF;
	rsp_buf[8 + 1] = (z >> 8) & 0xFF;
	rsp_buf[8 + 2] = (z >> 16) & 0xFF;
	rsp_buf[8 + 3] = (z >> 24) & 0xFF;

	*rsp_size = 12;
}

/*
 * buf x store device y
 * buf y store device x
 * buf z store device -z
 */
void HmdGyroRspPoll(uint8_t *rsp_buf, uint32_t *rsp_size)
{
	int x = 0;
	int y = 0;
	int z = 0;
	uint8_t buf_raw[6];

	MotionSensorGetGyro(buf_raw, sizeof(buf_raw));
	x = buf_raw[0];
	x |= buf_raw[1] << 8;
	x <<= 16;
	x >>= 16;
	//LogDebug("Gyr : x = %d ", x);
	if (x < -32767) {
		x = 0;
	}

	y = buf_raw[2];
	y |= buf_raw[3] << 8;
	y <<= 16;
	y >>= 16;
	//LogDebug("y = %d ", y);
	if (y < -32767) {
		y = 0;
	}

	z = buf_raw[4];
	z |= buf_raw[5] << 8;
	z <<= 16;
	z >>= 16;
	//LogDebug("z = %d\n", z);
	if (z < -32767) {
		z = 0;
	}

	x = -x;
	rsp_buf[0] = x & 0xFF;
	rsp_buf[1] = (x >> 8) & 0xFF;
	rsp_buf[2] = (x >> 16) & 0xFF;
	rsp_buf[3] = (x >> 24) & 0xFF;

	rsp_buf[4 + 0] = y & 0xFF;
	rsp_buf[4 + 1] = (y >> 8) & 0xFF;
	rsp_buf[4 + 2] = (y >> 16) & 0xFF;
	rsp_buf[4 + 3] = (y >> 24) & 0xFF;

	z = -z;
	rsp_buf[8 + 0] = z & 0xFF;
	rsp_buf[8 + 1] = (z >> 8) & 0xFF;
	rsp_buf[8 + 2] = (z >> 16) & 0xFF;
	rsp_buf[8 + 3] = (z >> 24) & 0xFF;

	*rsp_size = 12;
}

/*
 * buf x store device x
 * buf y store device -y
 * buf z store device -z
 */
void HmdMagneticRspPoll(uint8_t *rsp_buf, uint32_t *rsp_size)
{
	static int x = 0;
	static int y = 0;
	static int z = 0;
	uint8_t buf_raw[6];

	MotionSensorGetMag(buf_raw, sizeof(buf_raw));
	x = buf_raw[0];
	x |= buf_raw[1] << 8;
	x <<= 16;
	x >>= 16;
	x >>= 3;

	y = buf_raw[2];
	y |= buf_raw[3] << 8;
	y <<= 16;
	y >>= 16;
	y >>= 3;

	z = buf_raw[4];
	z |= buf_raw[5] << 8;
	z <<= 16;
	z >>= 16;
	z >>= 1;

	rsp_buf[0] = x & 0xFF;
	rsp_buf[1] = (x >> 8) & 0xFF;
	rsp_buf[2] = (x >> 16) & 0xFF;
	rsp_buf[3] = (x >> 24) & 0xFF;

	y = -y;
	rsp_buf[4 + 0] = y & 0xFF;
	rsp_buf[4 + 1] = (y >> 8) & 0xFF;
	rsp_buf[4 + 2] = (y >> 16) & 0xFF;
	rsp_buf[4 + 3] = (y >> 24) & 0xFF;

	z = -z;
	rsp_buf[8 + 0] = z & 0xFF;
	rsp_buf[8 + 1] = (z >> 8) & 0xFF;
	rsp_buf[8 + 2] = (z >> 16) & 0xFF;
	rsp_buf[8 + 3] = (z >> 24) & 0xFF;

	*rsp_size = 12;
}

void HmdEventRspPoll(uint8_t *rsp_buf, uint32_t *rsp_size)
{
	int ret;
	struct event_t key_event;
	int i = 0;

	do {
		TouchSensorDisable();
		ret = EventQueueDeQueue(&hmd_event_queue, &key_event);
		TouchSensorEnable();
		if (ret == 0) {
			rsp_buf[i + 0] = key_event.type & 0xFF;
			rsp_buf[i + 1] = (key_event.type >> 8) & 0xFF;
			rsp_buf[i + 2] = key_event.code & 0xFF;
			rsp_buf[i + 3] = (key_event.code >> 8) & 0xFF;
			rsp_buf[i + 4] = key_event.value & 0xFF;
			rsp_buf[i + 5] = (key_event.value >> 8) & 0xFF;
			rsp_buf[i + 6] = (key_event.value >> 16) & 0xFF;
			rsp_buf[i + 7] = (key_event.value >> 24) & 0xFF;
			i += 8;
			LogDebug("event type : %d, code : %d, value : %d\n", key_event.type, key_event.code, key_event.value);
		}
	} while ((ret == 0) && ((i + 8) <= 64));

	*rsp_size = i;
}

short calcChkSum(uint8_t *buffer, int byteOffset, int byteCount)
{
	short chkSum = 0;
	int byteEnd;
	int i;

	byteEnd = byteOffset + byteCount;
	for (i = byteOffset; i < byteEnd; i++) {
		chkSum += (short)(buffer[i] & 0x00FF);
	}
	return chkSum;
}

int pack(uint8_t *packedBuf, int packedCount, uint8_t *payloadBuf, int payloadOffset, int payloadCount) {
	int index;
	short chkSum;
	int i;

	if (packedCount < (payloadCount + 6)) {
		return -1;
	}

	index = 0;
	packedBuf[index] = HMD_CMD_DISP_XFER_SYNC_BEGIN;
	index++;
	packedBuf[index] = (payloadCount & 0x0000FF00) >> 8;
	index++;
	packedBuf[index] = (payloadCount & 0x000000FF);
	index++;
	for (i = 0; i < payloadCount; i++, index++) {
		packedBuf[index] = payloadBuf[payloadOffset + i];
	}
	chkSum = calcChkSum(packedBuf, 1, payloadCount + 2);
	packedBuf[index] = (chkSum & 0xFF00) >> 8;
	index++;
	packedBuf[index] = chkSum & 0x00FF;
	index++;
	packedBuf[index] = HMD_CMD_DISP_XFER_SYNC_END;
	index++;

	return index;
}

int unpack(uint8_t *packedBuf, int packedCount, uint8_t *payloadBuf, int payloadOffset, int payloadCount) {
	int index;
	short length;
	short chkSumRecv, chkSumCalc;
	int i;

	index = 0;
	if (packedBuf[index] != HMD_CMD_DISP_XFER_SYNC_BEGIN) {
		return -1;
	}
	index++;
	length = (short)((packedBuf[index] & 0x00FF) << 8);
	index++;
	length |= (short)(packedBuf[index] & 0x00FF);
	index++;
	if (packedCount != (length + 6)) {
		return -1;
	}
	if (payloadCount < length) {
		return -1;
	}
	for (i = 0; i < length; i++, index++) {
		payloadBuf[payloadOffset + i] = packedBuf[index];
	}
	chkSumCalc = calcChkSum(packedBuf, 1, length + 2);

	chkSumRecv = (short)((packedBuf[index] & 0x00FF) << 8);
	index++;
	chkSumRecv |= (short)(packedBuf[index] & 0x00FF);
	index++;

	if (chkSumRecv != chkSumCalc) {
		return -1;
	}
	if (packedBuf[index] != HMD_CMD_DISP_XFER_SYNC_END) {
		return -1;
	}

	return length;
}

int HmdDispRspXferError(uint8_t *sendCmdBuf, int sendCmdCount)
{
	int i;

	i = 0;
	sendCmdBuf[i] = HMD_CMD_DISP_XFER_RESULT_ERROR & 0xFF;
	i++;

	return i;
}

int HmdDispRspXferSetBrightness(uint8_t *sendCmdBuf, int sendCmdCount, uint8_t *recvCmdBuf, int recvCmdCount)
{
	int i;

	brightness_set_by_cmd = recvCmdBuf[1];
	is_brightness_changed_by_cmd = 1;

	i = 0;
	sendCmdBuf[i++] = HMD_CMD_DISP_XFER_RESULT_OK;
	sendCmdBuf[i++] = recvCmdBuf[1];
	sendCmdBuf[i++] = 0;
	sendCmdBuf[i++] = 0;
	sendCmdBuf[i++] = 0;

	return i;
}

int HmdDispRspXferGetBrightness(uint8_t *sendCmdBuf, int sendCmdCount, uint8_t *recvCmdBuf, int recvCmdCount)
{
	int i;

	i = 0;
	sendCmdBuf[i++] = HMD_CMD_DISP_XFER_RESULT_OK;
	sendCmdBuf[i++] = sys_config.brightness;
	sendCmdBuf[i++] = 0;
	sendCmdBuf[i++] = 0;
	sendCmdBuf[i++] = 0;

	return i;
}

int HmdDispRspXferGetVideoState(uint8_t *sendCmdBuf, int sendCmdCount, uint8_t *recvCmdBuf, int recvCmdCount)
{
	int i;
	uint8_t state;

	if (is_video_on) {
		state = HMD_PARAM_DISP_VIDEO_STATE_ON;
	} else {
		state = HMD_PARAM_DISP_VIDEO_STATE_OFF;
	}

	i = 0;
	sendCmdBuf[i] = HMD_CMD_DISP_XFER_RESULT_OK;
	i++;
	sendCmdBuf[i] = state;
	i++;

	return i;
}

int HmdDispRspXferGetVersion(uint8_t *sendCmdBuf, int sendCmdCount, uint8_t *recvCmdBuf, int recvCmdCount)
{
	int i;

	i = 0;
	sendCmdBuf[i] = HMD_CMD_DISP_XFER_RESULT_OK;
	i++;
	i += sprintf((char *)sendCmdBuf + i, "%u.%u", VERSION_MAJOR, VERSION_MINOR);

	return i;
}

void HmdDispRspXfer(uint8_t *cmd_buf, uint32_t cmd_size, uint8_t *rsp_buf, uint32_t rsp_size, uint32_t *rsp_ret_size)
{
	uint8_t sendCmdBuf[HMD_CMD_LENGTH_MAX - 6];
	int sendCmdCount;
	uint8_t recvCmdBuf[HMD_CMD_LENGTH_MAX - 6];
	int recvCmdCount;

	recvCmdCount = unpack(cmd_buf + 1, cmd_size - 1, recvCmdBuf, 0, sizeof(recvCmdBuf));
	sendCmdCount = HmdDispRspXferError(rsp_buf, sizeof(rsp_size));

	if (recvCmdCount < 0) {

	} else {
		switch (recvCmdBuf[0]) {
			case HMD_CMD_DISP_XFER_GET_VERSION :
				sendCmdCount = HmdDispRspXferGetVersion(sendCmdBuf, sizeof(sendCmdBuf), recvCmdBuf, recvCmdCount);
				break;
			case HMD_CMD_DISP_XFER_GET_VIDEO_STATE :
				sendCmdCount = HmdDispRspXferGetVideoState(sendCmdBuf, sizeof(sendCmdBuf), recvCmdBuf, recvCmdCount);
				break;
			case HMD_CMD_DISP_XFER_SET_BRIGHTNESS :
				sendCmdCount = HmdDispRspXferSetBrightness(sendCmdBuf, sizeof(sendCmdBuf), recvCmdBuf, recvCmdCount);
				break;
			case HMD_CMD_DISP_XFER_GET_BRIGHTNESS :
				sendCmdCount = HmdDispRspXferGetBrightness(sendCmdBuf, sizeof(sendCmdBuf), recvCmdBuf, recvCmdCount);
				break;
			default:
				break;
		}
	}

	*rsp_ret_size = pack(rsp_buf, rsp_size, sendCmdBuf, 0, sendCmdCount);
}

void HmdGyroHandleCmd(uint8_t *cmd_buf, uint32_t cmd_size, uint8_t *rsp_buf, uint32_t rsp_size, uint32_t *rsp_ret_size)
{
	if (cmd_size) {
		switch (cmd_buf[0]) {
			case HMD_CMD_GYRO_START:
				HmdRspNull(cmd_buf, cmd_size, rsp_buf, rsp_size, rsp_ret_size);
				break;
			case HMD_CMD_GYRO_STOP:
				HmdRspNull(cmd_buf, cmd_size, rsp_buf, rsp_size, rsp_ret_size);
				break;
			case HMD_CMD_GYRO_POLL:
				HmdGyroRspPoll(rsp_buf, rsp_ret_size);
				break;
			default:
				HmdRspNull(cmd_buf, cmd_size, rsp_buf, rsp_size, rsp_ret_size);
				break;
		}
	}
}

void HmdGsensorHandleCmd(uint8_t *cmd_buf, uint32_t cmd_size, uint8_t *rsp_buf, uint32_t rsp_size, uint32_t *rsp_ret_size)
{
	if (cmd_size) {
		switch (cmd_buf[0]) {
			case HMD_CMD_GSENSOR_START:
				HmdRspNull(cmd_buf, cmd_size, rsp_buf, rsp_size, rsp_ret_size);
				break;
			case HMD_CMD_GSENSOR_STOP:
				HmdRspNull(cmd_buf, cmd_size, rsp_buf, rsp_size, rsp_ret_size);
				break;
			case HMD_CMD_GSENSOR_POLL:
				HmdGsensorRspPoll(rsp_buf, rsp_ret_size);
				break;
			default:
				HmdRspNull(cmd_buf, cmd_size, rsp_buf, rsp_size, rsp_ret_size);
				break;
		}
	}
}

void HmdMagneticHandleCmd(uint8_t *cmd_buf, uint32_t cmd_size, uint8_t *rsp_buf, uint32_t rsp_size, uint32_t *rsp_ret_size)
{	
	switch (cmd_buf[0]) {
		case HMD_CMD_MAGNETIC_START:
			HmdRspNull(cmd_buf, cmd_size, rsp_buf, rsp_size, rsp_ret_size);
			break;
		case HMD_CMD_MAGNETIC_STOP:
			HmdRspNull(cmd_buf, cmd_size, rsp_buf, rsp_size, rsp_ret_size);
			break;
		case HMD_CMD_MAGNETIC_POLL:
			HmdMagneticRspPoll(rsp_buf, rsp_ret_size);
			break;
		case HMD_CMD_DISP_XFER:
			HmdDispRspXfer(cmd_buf, cmd_size, rsp_buf, rsp_size, rsp_ret_size);
			break;
		case HMD_CMD_EVENT_POLL:
			HmdEventRspPoll(rsp_buf, rsp_ret_size);
			break;
		default:
			HmdRspNull(cmd_buf, cmd_size, rsp_buf, rsp_size, rsp_ret_size);
			break;
	}
}
